import socket
import os
import subprocess
import select

# Source ROS环境
ros_setup_path = "/opt/ros/noetic/setup.bash"
mavros_ws_setup_path = "/home/wushuanglang/mavros_ws/devel/setup.bash"
# 动态 source setup.bash
command = ['bash', '-c', f"source {ros_setup_path} && source {mavros_ws_setup_path} && env"]
proc = subprocess.Popen(command, stdout=subprocess.PIPE)
for line in proc.stdout:
    (key, _, value) = line.decode("utf-8").partition("=")
    os.environ[key] = value.strip()

import rospy
from std_msgs.msg import String

# 定义服务器IP和端口
server_ip = '127.0.0.1'
server_port = 6666

# 创建 TCP socket
client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
client_socket.setblocking(False)  # 设置为非阻塞模式

def start_client():
    # 连接到服务器
    try:
        client_socket.connect((server_ip, server_port))
    except BlockingIOError:
        # 在非阻塞模式下，连接可能会立即失败
        pass

    while True:
        # 发送数据
        message = input("请输入要发送的消息 (输入 'exit' 退出): ")
        if message == 'exit':
            print("客户端退出...")
            break

        try:
            client_socket.send(message.encode())
        except BlockingIOError:
            print("发送数据时阻塞，请稍后再试。")

        # 使用 select 监听 socket 是否可读
        readable, _, _ = select.select([client_socket], [], [], 0.1)
        if readable:
            try:
                # 接收服务器的确认信息
                response = client_socket.recv(1024)
                print(f"收到来自服务器的确认: {response.decode()}")
            except BlockingIOError: # 非阻塞模式下，如果没有数据可读，调用 recv 方法将抛出异常。因此，我们需要处理这种情况
                print("接收数据时阻塞，请稍后再试。")

    # 关闭连接
    client_socket.close()

if __name__ == "__main__":
    start_client()
